using RobotSimulator.Basic;
using System;
using System.Collections;
using System.Collections.Generic;
using Unity.VisualScripting;
using UnityEngine;
public class FourRotor : BasicRobot
{
    [HideInInspector]
    public IMU imu;
    [HideInInspector]
    public Altimeter altimeter;
    [HideInInspector]
    public GPS gps;
    protected override void Start()
    {
        base.Start();
        foreach (var sensor in this.sensors)
        {
            if (sensor is IMU)
            {
                imu = (IMU)sensor;
            }
            else if (sensor is Altimeter)
            {
                altimeter = (Altimeter)sensor;
            }
            else if (sensor is GPS)
            {
                gps = (GPS)sensor;
            }
        }
    }
}
public class CmdFourRotorControlAccelerate : Command
{
    public float[] acc;

    public string FourRotorControlAccelerate(IEU ieu)
    {
        if(ieu is not FourRotor)
        {
            return Command.ErrorText;
        }
        FourRotor fr= (FourRotor)ieu;
        for(int i=0; i < acc.Length; i++)
        {
            fr.actuators[i].RecvCmd(new CmdPropellerAddForce(acc[i] / fr.Mass));
        }
        return "ok";
    }

    public CmdFourRotorControlAccelerate(float acc0,float acc1,float acc2,float acc3)
    {
        acc = new float[4];
        acc[0] = acc0;
        acc[1] = acc1;
        acc[2] = acc2;
        acc[3] = acc3;
        this.execute = FourRotorControlAccelerate;
    }
    public CmdFourRotorControlAccelerate(float[] acc)
    {
        this.acc = acc;
        this.execute = FourRotorControlAccelerate;
    }
}